/* Helsinki University of Technology
 * T-106.530 Embedded Systems
 *
 * Car-project
 * HW interface
 *
 * Target: Atmel AVR ATmega32
 *
 * Author: Petri Hintukainen, 44020U
 * Date:   2012/03/16
 */

#include <avr/interrupt.h>

#include "hw.h"
#include "hwdefs.h"

#define SET_BIT_HI(port, mask) port |= (mask)
#define SET_BIT_LO(port, mask) port &= (~(mask))

#define nop()   __asm__ __volatile__("nop"::)


/**********************************************
 * HW Interface
 **********************************************/

/*
 * Current time
 */

volatile uint64_t ticks;

ISR(TIMER2_COMP_vect)
{
  //ticks++;
}

ISR(TIMER2_OVF_vect)
{
  ticks++;
}

uint64_t hw_time_us(void)
{
  return ticks * TICK_US;
}

uint32_t hw_time_ms(void)
{
  return ticks * TICK_US / 1000;
}

/*
static void _hw_wait_us(int64_t ticks)
{
  uint64_t time = hw_time_us() + ticks;
  while (hw_time_us() < time);
}
*/

static void _hw_sleep_tick(void)
{
  /* wait at least one tick (=> 128...256 us) */
  int64_t t = ticks + 2;
  while (ticks < t);
}

/*
 * LCD display
 */

#define LCD_BIT_HI(mask) SET_BIT_HI(LCD_PORT_OUT, (mask))
#define LCD_BIT_LO(mask) SET_BIT_LO(LCD_PORT_OUT, (mask))

/*
 * RS - register select - 0 command, 1 data
 * EN - clock enable, falling edge enabled
 */

static void _lcd_clock(void)
{
  LCD_BIT_HI(LCD_EN_MASK);

  _hw_sleep_tick();

  LCD_BIT_LO(LCD_EN_MASK);

  _hw_sleep_tick();
}

static void _lcd_send_nibble(uint8_t data, int command)
{
  LCD_BIT_LO(LCD_PORT_MASK);
  LCD_PORT_OUT |= ((data << LCD_DATA_SHIFT) & LCD_DATA_MASK) | (command ? 0 : LCD_RS_MASK);
  _lcd_clock();
}

static void _lcd_send(uint8_t data, int command)
{
  _lcd_send_nibble(data>>4, command);
  _lcd_send_nibble(data,    command);
}

void hw_lcd_byte(uint8_t c)
{
  _lcd_send(c, 0);
}

void hw_lcd_ctrl(uint8_t c)
{
  _lcd_send(c, 1);
}

/*
 * LED and button
 */

int hw_read_button(void)
{
  return !(BUTTON_PORT_IN & BUTTON_PORT_MASK);
}

void hw_set_led(int state)
{
  cli();

  if (state) {
    LED_PORT_OUT |= LED_PORT_MASK;
  } else {
    LED_PORT_OUT &= ~LED_PORT_MASK;
  }

  sei();
}

/*
 * Servo
 */

/*
 * servo: 20ms pulse perioid
 *   0.75 ... 2.25 ms pulse width: -85...+85 degrees
 * The servo is connected to the B output of the Timer/Counter1
 * subsystem (Port D pin 4 , OC1B)
 */

volatile int servo_pwm_count   = 0;  /* timer counter */
volatile int servo_pwm_done_at = 0;
volatile int servo_pwm_done    = 0;

static void _stop_servo_pwm(void)
{
  TIMSK  &= ~(1 << OCIE1B); // Disable interrupt
  TCCR1A = 0;
  TCCR1B = 0;

  servo_pwm_done  = 1;
  servo_pwm_count = 0;
}

ISR(TIMER1_COMPB_vect)
{
  if (++servo_pwm_count > servo_pwm_done_at) {
    _stop_servo_pwm();
  }
}

void hw_servo_set(uint8_t angle /* angle: 0 ... 255 */)
{
  cli();

  TCCR1A = 0;
  TCCR1B = 0;
  // COM1A1: Clear OC1A/OC1B on compare match, set OC1A/OC1B at BOTTOM 
  // WGM:    fast pwm, mode 14
  // CS:     1/8 clk io
  TCCR1A |= (1<<COM1B1) | (1<<WGM11);
  TCCR1B |= /*(1<<CS10) |*/ (1<<CS11) | (1<<WGM13) | (1<<WGM12);

  // 20 ms (50 Hz) pulse perioid
  ICR1  = SYSTEM_CLOCK/(8*50);  // Set CTC compare value 20ms

// start: 0.75 ms
  const uint32_t ticks_in_ms = SYSTEM_CLOCK/(8*1000);
  const uint16_t start = ticks_in_ms * 750/1000;
// end: 2.25 ms
  const uint16_t end   = SYSTEM_CLOCK/(8*1000) * 2250/1000;
// max width
  const uint16_t max_w = end - start;

  // configurable pulse width (angle = 0...100)
  // 1 tick = 64 us
  // .75 ... 2.25 ms
  // 750...2250 us = 750/64 ... 2250/64 ticks

  //OCR1A = 8*11 + (8*24*angle/100);
  // = 11...35 ticks
//  OCR1B = 8*11 + (8*24*angle/100);
#warning fix this
  OCR1B = 16 * ( 8*13 + (8*20*angle/256) );

  // pwm active for 3 sec (20ms*50*3)
  servo_pwm_done_at = 3 * 50;

  // reset counter and flag
  servo_pwm_done = servo_pwm_count = 0;
  sei();
  TIMSK |= (1 << OCIE1B); // Enable CTC interrupt
}

/*
 * Motor
 */

static void _motor_pwm_init(void)
{
  cli();

  TCCR2 = 0;

  /* WGM: mode 3, fast pwm
   * COM: Clear OC2 on compare match, set on bottom
   * CS: 1:8 CLKt2s
   *
   * => 7812 Hz PWM signal
   */
  TCCR2 = (1<<WGM21) | (1<<WGM20) | (1<<COM21) /*| (1<<CS22) */| (1<<CS21) /*| (1<<CS20)*/;
  OCR2  = 0; // pulse width, 0...0xff

  sei();

  /* overflow interrupt is used to generate system time */

  TIMSK |= (1 << TOIE2); // Enable overflow interrupt
}

static uint8_t prev_dir = MOTOR_CTRL_BRAKE_GND;

static void _motor_set_dir(uint8_t dir)
{
  cli();

  prev_dir = MOTOR_CTRL_OUT & MOTOR_CTRL_MASK;

  MOTOR_CTRL_OUT = (MOTOR_CTRL_OUT & (~MOTOR_CTRL_MASK)) | dir;

  sei();
}

static void _motor_set_speed(uint8_t speed)
{
  OCR2 = speed;
}

int16_t hw_motor_speed(int16_t speed)
{
  uint8_t dir, abs_speed;

  if (speed < 0) {
    dir = MOTOR_CTRL_CCW;
    abs_speed = -speed;

  } else if (speed == 0) {
    dir = MOTOR_CTRL_BRAKE_GND;
    abs_speed = 0;

  } else /* if (speed > 0) */ {
    dir = MOTOR_CTRL_CW;
    abs_speed = speed;
  }

  uint8_t current_dir = MOTOR_CTRL_OUT & MOTOR_CTRL_MASK;
  if (dir != current_dir) {
    /* need to stop before changing direction ... ? */
  }
 
  _motor_set_dir(dir);
  _motor_set_speed(abs_speed);

  return hw_current_speed();
}

int16_t hw_current_speed(void)
{
  // read tacometer

  // use prev_dir to detect direction if brake on

  return 0;
}

/*
 * Sensors
 */

unsigned int hw_ir_state(void)
{
  return SENSOR_PORT_IN & SENSOR_PORT_MASK;
}

/*
 * Initialize hardware
 */

void hw_init(void)
{
  /* button */
  BUTTON_PORT_DDR &= ~BUTTON_PORT_MASK; /* set pin to input */
  BUTTON_PORT_OUT |= BUTTON_PORT_MASK;  /* pull-up resistor on */

  /* led */
  LED_PORT_DDR |= LED_PORT_MASK;        /* set pin to output */
  LED_PORT_OUT &= ~LED_PORT_MASK;       /* LED off */

  /* LCD display */
  LCD_PORT_DDR |= LCD_PORT_MASK;        /* set pin to output */
  LCD_PORT_OUT &= ~LCD_PORT_MASK;       /* output low */

  /* servo */
  SERVO_PORT_DDR |= SERVO_PORT_MASK;    /* set pin to output */
  SERVO_PORT_OUT &= ~SERVO_PORT_MASK;   /* output low */

  /* motor PWM */
  MOTOR_PWM_DDR |= MOTOR_PWM_MASK;      /* set pin to output */
  MOTOR_PWM_OUT &= ~MOTOR_PWM_MASK;     /* output low */

  /* motor control */
  MOTOR_CTRL_DDR |= MOTOR_CTRL_MASK;    /* set pin to output */
  MOTOR_CTRL_OUT = (MOTOR_CTRL_OUT & (~MOTOR_CTRL_MASK)) | MOTOR_CTRL_BRAKE_GND;  /* motor off */

MOTOR_CTRL_OUT |= MOTOR_CTRL_CCW;

  /* motor diagnostic */
  MOTOR_DIAG_DDR &= ~MOTOR_DIAG_MASK;   /* set pin to input */
  MOTOR_DIAG_OUT &= ~MOTOR_DIAG_MASK;   /* pull-up resistor off */
#if 0
  /* Motor tachometer */
  MOTOR_TM_DDR1 &= ~MOTOR_TM_MASK1;     /* set pins to input */
  MOTOR_TM_DDR2 &= ~MOTOR_TM_MASK2;     /* set pins to input */
  MOTOR_TM_OUT1 &= ~MOTOR_TM_MASK1;     /* pull-up resistors off */
  MOTOR_TM_OUT2 &= ~MOTOR_TM_MASK2;     /* pull-up resistors off */

  /* IR sensors */
  SENSOR_PORT_DDR = (uint8_t)~SENSOR_PORT_MASK;  /* set pins to input */
  SENSOR_PORT_OUT = (uint8_t)~SENSOR_PORT_MASK;  /* pull-up resistors off */
#endif
  /* current time */
  ticks = 0;

  _motor_pwm_init();
  //_lcd_init();
}

